#include "HXSY.h"


void HXSY::HX_MP3_VOL(unsigned char vo)
{
	delay(5);
	Serial.write(0XAA);	
	Serial.write(0X13);
	Serial.write(0X01);
	Serial.write(vo);
	Serial.write(0XAA + 0X13 + 0X01 + vo);
	delay(5);
		
}
void HXSY::HX_MP3_play(unsigned int vo)
{
	unsigned char a,b;
	
	a = (vo >> 8)&0xff;
	b = vo&0xff;
	delay(5);
	Serial.write(0XAA);	
	Serial.write(0X07);
	Serial.write(0X02);
	Serial.write(a);
	Serial.write(b);
	Serial.write(0XAA + 0X07 + 0X02 + a + b);
	delay(5);
}

unsigned char HXSY::HXGETDISTENCE(unsigned char d)
{
	unsigned char buff ;
	unsigned int cnt;
	cs100Abuff[0] = 0XAF;
	cs100Abuff[1] = 0x00;
	cs100Abuff[2] = 0x00;
	cs100Abuff[3] = 0x00;
	
	CS100A_SEND(d);
	
	  pinMode(d,INPUT);
	  delayMicroseconds(10) ;
	  cnt = 0;
	  while(digitalRead(d) == 1)
	  {
		  delayMicroseconds(1);
		  cnt++;
		  if(cnt > 10000)
		  {
			return 0;   
		  }
		  
	  }
	    delayMicroseconds(30) ;//延时100us，开始传递一个字节
	    for(char i=0;i<8;i++)
	    {
	        buff = buff<<1;
	        if (digitalRead(d) == 1)//总线如果被拉高,该位设为1
	        {
	            buff |= 1;   
	        }  
	        delayMicroseconds(100);//等待下一位的信号
	    }
	  delay(10);
	 
	  return buff;
	
}
void HXSY::HXsetColor(unsigned char d,unsigned char s,long int color)
{
   unsigned char buff1;

	if(s == 0)
	{
		cs100Abuff[0] = 0XA3;	
	}
	else if(s == 1)
	{
		cs100Abuff[0] = 0XAC;	
	}
	
	
	cs100Abuff[1] = color & 0xFF;
	cs100Abuff[2] = (color >> 8)&0xFF;
	cs100Abuff[3] = (color >>16)&0xFF;
	
	CS100A_SEND(d);
	delay(10) ;
}
bool HXSY::LIGHT_B_W(unsigned char aa,unsigned char bb,unsigned char cc)
{
	char buff = 0;
	
	  pinMode(aa, OUTPUT);
	  digitalWrite(aa, LOW);//拉低总线，开始通信
	  pinMode(aa, INPUT);
	  delayMicroseconds (130) ;//延时110us
	  if (digitalRead(aa) == 1)//总线如果被拉高，那么开始通信
	  {
	    delayMicroseconds (100) ;//延时100us，开始传递一个字节
	    for(char i=0;i<8;i++)
	    {
	        buff = buff<<1;
	        if (digitalRead(aa) == 1)//总线如果被拉高,该位设为1
	        {
	            buff |= 1;   
	        }  
	        delayMicroseconds (100) ;//等待下一位的信号
	    }
	  }
	  if(cc == 1)//黑
	  {
			if(bb == 0)//左
            {
				if((buff & 0x01) == 0x01)	 return 1;
				else  return 0;
			}
			if(bb == 1)//右
			{
				if((buff & 0x02) == 0x02)	 return 1;
				else  return 0;
			} 
	  }
	  if(cc == 0)//白
	  {
	  			if(bb == 0)//左
	            {
	  				if((buff & 0x01) == 0x00)	 return 1;
	  				else  return 0;
	  			}
	  			if(bb == 1)//右
	  			{
	  				if((buff & 0x02) == 0x00)	 return 1;
	  				else  return 0;
	  			} 
	  }
	
}
bool HXSY::LIGHT5_B_W(unsigned char aa,unsigned char bb,unsigned char cc)
{
	char buff = 0;
	
	  pinMode(aa, OUTPUT);
	  digitalWrite(aa, LOW);//拉低总线，开始通信
	  pinMode(aa, INPUT);
	  delayMicroseconds (130) ;//延时110us
	  if (digitalRead(aa) == 1)//总线如果被拉高，那么开始通信
	  {
	    delayMicroseconds (100) ;//延时100us，开始传递一个字节
	    for(char i=0;i<8;i++)
	    {
	        buff = buff<<1;
	        if (digitalRead(aa) == 1)//总线如果被拉高,该位设为1
	        {
	            buff |= 1;   
	        }  
	        delayMicroseconds (100) ;//等待下一位的信号
	    }
	  }
	  if(cc == 1)//黑
	  {
			if(bb == 1)//1
            {
				if((buff & 0x01) == 0x01)	 return 1;
				else  return 0;
			}
			else if(bb == 2)
			{
				if((buff & 0x02) == 0x02)	 return 1;
				else  return 0;	
			}
			else if(bb == 3)
			{
				if((buff & 0x04) == 0x04)	 return 1;
				else  return 0;	
			}
			else if(bb == 4)
			{
				if((buff & 0x08) == 0x08)	 return 1;
				else  return 0;	
			}
			else if(bb == 5)
			{
				if((buff & 0x10) == 0x10)	 return 1;
				else  return 0;	
			}
	  }
	  if(cc == 0)//白
	  {
	  		if(bb == 1)//1
	         {
	  		     if((buff & 0x01) == 0x00)	 return 1;
	  			 else  return 0;
	  		 }
			 else if(bb == 2)
	         {
	  		     if((buff & 0x02) == 0x00)	 return 1;
	  			 else  return 0;
	  		 }
			 else if(bb == 3)
			 {
			     if((buff & 0x04) == 0x00)	 return 1;
			 	 else  return 0;
			 }
			 else if(bb == 4)
			 {
			     if((buff & 0x08) == 0x00)	 return 1;
			 	 else  return 0;
			 }
			 else if(bb == 5)
			 {
			     if((buff & 0x10) == 0x00)	 return 1;
			 	 else  return 0;
			 }
	  }
	
}
void HXSY::WriteSEG(unsigned char KEY2,unsigned int SEGdat)
{
	SEG[0] = SEGdat/1000%10;
	SEG[1] = SEGdat/100%10;
	SEG[2] = SEGdat/10%10;
	SEG[3] = SEGdat%10;
	
	pinMode(KEY2, OUTPUT);
	digitalWrite(KEY2, LOW);//拉低总线，请求通信
	delayMicroseconds (20) ; 
	  for(char i=0;i<4;i++)
	  {
	       digitalWrite(KEY2, LOW);//拉低总线，请求通信
	       delayMicroseconds (100) ;
	       digitalWrite(KEY2, HIGH);//拉低总线，请求通信
		   delayMicroseconds (2) ;
	       segbuff = SEG[i];
	        for(char j=0;j<8;j++)
	        {
	            if(segbuff & 0x80)
	            {
	                digitalWrite(KEY2, HIGH); 
	                delayMicroseconds (100) ;//延时100us
	            }
	            else
	            {
	                digitalWrite(KEY2, LOW); 
	                delayMicroseconds (100) ;//延时100us
	            }
	            segbuff = segbuff<<1; 
	        }     
	  }
	delayMicroseconds (500) ;
	
}
void HXSY::waitbutton(unsigned char KEY1,unsigned char swKEY1)
{
	while(buttonPressed(KEY1,swKEY1)  == 1)	
	{
		delayMicroseconds (100) ;		
	}
	
}
bool HXSY::buttonPressed(unsigned char KEY,unsigned char swKEY)
{
	  char buff = 0;
	
	  pinMode(KEY, OUTPUT);
	  digitalWrite(KEY, LOW);//拉低总线，开始通信
	  pinMode(KEY, INPUT);
	  delayMicroseconds (130) ;//延时110us
	  if (digitalRead(KEY) == 1)//总线如果被拉高，那么开始通信
	  {
	    delayMicroseconds (65) ;//延时100us，开始传递一个字节
	    for(char i=0;i<8;i++)
	    {
	        buff = buff<<1;
	        if (digitalRead(KEY) == 1)//总线如果被拉高,该位设为1
	        {
	            buff |= 1;   
	        }  
	        delayMicroseconds (101) ;//等待下一位的信号
	    }
	  }
        if(swKEY == 1)
		{
			if((buff & 0XAF)== 0xa7) 
			 {  return 1;}
			else
			{   return 0;}
	    }
		if(swKEY == 2)
		{
			if((buff & 0XAF)== 0xaB) 
			 {  return 1;}
			else
			{   return 0;}
		}
		if(swKEY == 3)
		{
			if((buff & 0XAF)== 0xaD) 
			 {  return 1;}
			else
			{   return 0;}
		}
		if(swKEY == 4)
		{
			if((buff & 0XAF)== 0xaE) 
			 {  return 1;}
			else
			{   return 0;}
		}
			

	
}

void HXSY::MOTOR_STOP_ONE(unsigned char aaa)
{
     if(aaa == 0)
	 {
		ledcWrite(0,0);ledcWrite(1,0);	 		 
	 }
	 if(aaa == 1)
	 {
	 	ledcWrite(2,0);ledcWrite(3,0);		 
	 }
	 if(aaa == 2)
	 {
	 	ledcWrite(4,0);ledcWrite(5,0);		 
	 }
	 if(aaa == 3)
	 {
	 	ledcWrite(6,0);ledcWrite(7,0);		 
	 }
}
void HXSY::MOTOR_STOP()
{
	ledcWrite(0,0);ledcWrite(1,0);	 
	ledcWrite(2,0);ledcWrite(3,0);	 
	ledcWrite(4,0);ledcWrite(5,0);	 
	ledcWrite(6,0);ledcWrite(7,0);	 

}
void HXSY::MOTOR_SEND_START(unsigned char aa,int bb)
{  
	unsigned int speed;
	unsigned int fbb;
	fbb = bb * -1 ;
	

	
	if(bb>1)
	{
		speed = map(bb, 0, 100, 0, 1000);
		if(aa == 0)
		{
			ledcWrite(0,0);ledcWrite(1,speed);
		}
		else if(aa == 1)
		{
			ledcWrite(2,0);ledcWrite(3,speed);
		}
		else if(aa == 2)
		{
			ledcWrite(4,0);ledcWrite(5,speed);
		}
		else if(aa == 3)
		{
			ledcWrite(6,0);ledcWrite(7,speed);
		}
		
	}
	else if(bb < -1)
	{
		speed = map(fbb, 0, 100, 0, 1000);
		if(aa == 0)
		{
			ledcWrite(1,0);ledcWrite(0,speed);
		}	
		else if(aa == 1)
		{
			ledcWrite(3,0);ledcWrite(2,speed);
		}
		else if(aa == 2)
		{
			ledcWrite(5,0);ledcWrite(4,speed);
		}
		else if(aa == 3)
		{
			ledcWrite(7,0);ledcWrite(6,speed);
		}
		
	}
	else
	{
		if(aa == 0)
		{
				ledcWrite(0,0);ledcWrite(1,0);	 		 
		}
		if(aa == 1)
		{
			ledcWrite(2,0);ledcWrite(3,0);		 
		}
		if(aa == 2)
		{
			ledcWrite(4,0);ledcWrite(5,0);		 
		}
		if(aa == 3)
		{
			ledcWrite(6,0);ledcWrite(7,0);		 
		}
	}
}
void HXSY::MOTOR4_SEND_START(int a1,int a2,int a3,int a4)
{  
	unsigned int speedA1,speedZ1,speedA2,speedZ2,speedA3,speedZ3,speedA4,speedZ4;	
	int fsp1,fsp2,fsp3,fsp4;
	
	
	fsp1 = (a1* -1);
	fsp2 = (a2* -1);
	fsp3 = (a3* -1);
	fsp4 = (a4* -1);
	
	speedA1 = map(a1, 0, 100, 0, 1000);
	speedA2 = map(a2, 0, 100, 0, 1000);
	speedA3 = map(a3, 0, 100, 0, 1000);
	speedA4 = map(a4, 0, 100, 0, 1000);
	
	speedZ1 = map(fsp1, 0, 100, 0, 1000);
	speedZ2 = map(fsp2, 0, 100, 0, 1000);
	speedZ3 = map(fsp3, 0, 100, 0, 1000);
	speedZ4 = map(fsp4, 0, 100, 0, 1000);
	
	if(a1>=0)
	{	ledcWrite(0,0);ledcWrite(1,speedA1);	}
	else if(a1<0)
	{	ledcWrite(1,0);ledcWrite(0,speedZ1);	}
	
    if(a2>=0)
	{	ledcWrite(2,0);ledcWrite(3,speedA2);	}
	else if(a2<0)
	{	ledcWrite(3,0);ledcWrite(2,speedZ2);	}

	if(a3>=0)
	{	ledcWrite(4,0);ledcWrite(5,speedA3);	}
	else if(a3<0)
	{	ledcWrite(5,0);ledcWrite(4,speedZ3);	}

	if(a4>=0)
	{	ledcWrite(6,0);ledcWrite(7,speedA4);	}
	else if(a4<0)
	{	ledcWrite(7,0);ledcWrite(6,speedZ4);	}
}
void HXSY::MOTORCD_SEND_START(int a3,int a4)
{  
	unsigned int speedA3,speedZ3,speedA4,speedZ4;	
	int  fsp3,fsp4;
	int  cnt,cnt1;
	
	
	
	fsp3 = (a3* -1);
	fsp4 = (a4* -1);
	
	speedA3 = map(a3, 0, 100, 0, 1000);
	speedA4 = map(a4, 0, 100, 0, 1000);
	
	speedZ3 = map(fsp3, 0, 100, 0, 1000);
	speedZ4 = map(fsp4, 0, 100, 0, 1000);
/*
    if(a3 <=0  && a4 >=0)
	{
		cnt1 =0;
		if(speedZ3 > speedA4)  
		  {cnt = speedZ3;	}
		else       
		  { cnt = speedA4;}

		ledcWrite(5,0);	ledcWrite(6,0);
		while(cnt1<cnt)
		{
			cnt1++;
			if(cnt1 < speedZ3)
			  {ledcWrite(4,cnt1);}
			if(cnt1 < speedA4)
			  {ledcWrite(7,cnt1);}
			delayMicroseconds (1) ;
		}
		ledcWrite(4,speedZ3);
        ledcWrite(7,speedA4);
	}
    else if(a3 <=0  && a4 <=0)
	{
		cnt1 =0;
		if(speedZ3 > speedZ4)  
		  {cnt = speedZ3;	}
		else       
		  { cnt = speedZ4;}

		ledcWrite(5,0);	ledcWrite(7,0);
		while(cnt1<cnt)
		{
			cnt1++;
			if(cnt1 < speedZ3)
			  {ledcWrite(4,cnt1);}
			if(cnt1 < speedZ4)
			  {ledcWrite(6,cnt1);}
			delayMicroseconds (1) ;
		}
        ledcWrite(4,speedZ3);
        ledcWrite(6,speedZ4);
	}
	else if(a3 >=0  && a4 >=0)
	{
		cnt1 =0;

		if(speedA3 > speedA4)  
		  {cnt = speedA3;	}
		else       
		  { cnt = speedA4;}

		ledcWrite(4,0);	ledcWrite(6,0);
		while(cnt1<cnt)
		{
			cnt1++;
			if(cnt1 < speedA3)
			  {ledcWrite(5,cnt1);}
			if(cnt1 < speedA4)
			  {ledcWrite(7,cnt1);}
			delayMicroseconds (1) ;
		}
		ledcWrite(5,speedA3);
		ledcWrite(7,speedA4);

	}
	else if(a3 >=0  && a4 <=0)
	{
		cnt1 =0;

		if(speedA3 > speedZ4)  
		  {cnt = speedA3;	}
		else       
		  { cnt = speedZ4;}

		ledcWrite(4,0);	ledcWrite(7,0);
		while(cnt1<cnt)
		{
			cnt1++;
			if(cnt1 < speedA3)
			  {ledcWrite(5,cnt1);}
			if(cnt1 < speedZ4)
			  {ledcWrite(6,cnt1);}
			delayMicroseconds (1) ;
		}
		ledcWrite(5,speedA3);
		ledcWrite(6,speedZ4);

	}
*/

	if(a3>=0)
	{	ledcWrite(4,0);ledcWrite(5,speedA3);	}
	else if(a3<0)
	{	ledcWrite(5,0);ledcWrite(4,speedZ3);	}

	if(a4>=0)
	{	ledcWrite(6,0);ledcWrite(7,speedA4);	}
	else if(a4<0)
	{	ledcWrite(7,0);ledcWrite(6,speedZ4);	}
	
}

int HXSY::GETBMDJ(char sw)
{
	if(sw == 0)
	{
			return current_rightPosition;
		
	}
	if(sw == 1)
	{
			return current_leftPosition;
		
	}
	
}
void HXSY::findBM(char sw)
{
	if(sw == 0)
	{
		if ((digitalRead(2))) {
				while (!((digitalRead(2))==0)) {}
				if ((digitalRead(0))) {
					current_rightPosition += 1;
				}
				else {
					current_rightPosition -= 1;
				}
			}	
	}
	else
	{
		if ((digitalRead(5))) {
				while (!((digitalRead(5))==0)) {}
				if ((digitalRead(13))) {
					current_leftPosition += 1;
				}
				else {
					current_leftPosition -= 1;
				}
			}	
		
	}
	
}
	 
void HXSY::MOTOR_steup()
{
	ledcSetup(0,1000,10);
	ledcSetup(1,1000,10);
	ledcSetup(2,1000,10);
	ledcSetup(3,1000,10);
	ledcSetup(4,1000,10);
	ledcSetup(5,1000,10);
	ledcSetup(6,1000,10);
	ledcSetup(7,1000,10);
	
	ledcAttachPin(2,0);
	ledcAttachPin(0,1);
	ledcAttachPin(13,2);
	ledcAttachPin(5,3);
	ledcAttachPin(25,4);
	ledcAttachPin(15,5);
	ledcAttachPin(27,6);
	ledcAttachPin(26,7);
	
}
void HXSY::MOTOR_step()
{
	ledcSetup(4,1000,10);
	ledcSetup(5,1000,10);
	ledcSetup(6,1000,10);
	ledcSetup(7,1000,10);
	

	ledcAttachPin(25,4);
	ledcAttachPin(15,5);
	ledcAttachPin(27,6);
	ledcAttachPin(26,7);
	
}
